from machine import PWM, Pin


class ESC:
    def __init__(self, mode):
        # mode  0为空模电调 1位车模电调
        self.speed_standby_duty_ns = 1501000 if mode else 1000000  # 遥控中位占空比 空模1ms 车模1.5ms
        self.speed_max_duty_ns = 2000000  # 油门最高占空比 约为2ms
        self.speed_min_duty_ns = 1000000  # 油门最低占空比 约为1ms
        self.speed_control_duty_ns = 20000  # 控制占空比 默认为0.02ms
        self.esc_pwm = [PWM(Pin(17)), PWM(Pin(18)), PWM(Pin(19)), PWM(Pin(20))]  # 电调pwm控制
        for esc in self.esc_pwm:
            esc.freq(50)  # 默认电调是基于50hz的pwm调速
            esc.duty_ns(self.speed_standby_duty_ns)  # 默认将所有电调油门配置为中位

    # 设置油门速度
    def setControlFreq(self, channel, freq):
        self.esc_pwm[channel].freq(freq)

    # 获取油门速度
    def getControlFreq(self, channel):
        return self.esc_pwm[channel].freq()

    # 设置油门速度
    def setSpeed(self, channel, speed):
        self.esc_pwm[channel].duty_ns(self.speed_min_duty_ns + int(float(speed) * self.speed_control_duty_ns))

    # 获取油门速度
    def getSpeed(self, channel):
        return self.esc_pwm[channel].duty_ns()

    # 设置所有油门速度
    def setAllSpeed(self, speed):
        for esc in self.esc_pwm:
            esc.duty_ns(self.speed_min_duty_ns + int(float(speed) * self.speed_control_duty_ns))

# def esc_init(esc_num):
#     esc_pwm[esc_num].duty_ns(speed_max_duty_ns)
#     time.sleep(2)
#     esc_pwm[esc_num].duty_ns(speed_standby_duty_ns)
#     time.sleep(1)
